#include "devmem2.h"

#define FATAL                                                    \
    do                                                           \
    {                                                            \
        fprintf(stderr, "Error at line %d, file %s (%d) [%s]\n", \
                __LINE__, __FILE__, errno, strerror(errno));     \
        exit(1);                                                 \
    } while (0)

#define MAP_SIZE 4096UL
#define MAP_MASK (MAP_SIZE - 1)

static inline void *fixup_addr(void *addr, size_t size);


static inline void *fixup_addr(void *addr, size_t size)
{
#ifdef FORCE_STRICT_ALIGNMENT
    unsigned long aligned_addr = (unsigned long)addr;
    aligned_addr &= ~(size - 1);
    addr = (void *)aligned_addr;
#endif
    return addr;
}



unsigned long devmem_read(off_t addr)
{
    int fd;
    void *map_base,*virt_addr;
	size_t data_size;
    //读取到的数据
    uint64_t read_result;
    //目标地址
    off_t target_base;

    if ((fd = open(FILE_PATH, O_RDWR | O_SYNC)) == -1)
    {
        return -1;
    }
    fflush(stdout);

    //对该地址进行映射
    map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, addr & ~MAP_MASK);

    virt_addr = map_base + (addr & MAP_MASK);

    if (map_base == (void *)-1)
        return -1;
    fflush(stdout);

    //获取数据
 	data_size = sizeof(uint32_t);
    virt_addr = fixup_addr(virt_addr, data_size);
    read_result = *((uint32_t *)(virt_addr));

    fflush(stdout);

    if (munmap(map_base, MAP_SIZE) == -1)
        return -1;
    close(fd);
    return read_result;
}

unsigned long devmem_write(off_t addr, unsigned long val)
{
    int fd;
    void *map_base,*virt_addr;
	size_t data_size;
    //读取到的数据
    uint64_t read_result;
    //目标地址
    off_t target_base;

    if ((fd = open(FILE_PATH, O_RDWR | O_SYNC)) == -1)
    {
        return -1;
    }
    fflush(stdout);

    //对该地址进行映射
    map_base = mmap(0, MAP_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, fd, addr & ~MAP_MASK);

    virt_addr = map_base + (addr & MAP_MASK);

    if (map_base == (void *)-1)
        return -1;
    fflush(stdout);

    //获取数据
 	data_size = sizeof(uint32_t);
    virt_addr = fixup_addr(virt_addr, data_size);
    *((uint32_t *)virt_addr) = val;
    read_result = *((uint32_t *)(virt_addr));

    fflush(stdout);

    if (munmap(map_base, MAP_SIZE) == -1)
        return -1;
    close(fd);
    return read_result;
}


/*
 *  配置gpio复用 
 *  @addr: 寄存器地址
 *  @offset: 寄存器偏移位数
 *  @val: 要写入的值
*/
void devmem_gpio_remap(off_t addr,int offset, uint32_t val)
{
    uint32_t a = 3;
    unsigned long read_val;
    read_val = devmem_read(addr+GPIO_REMAP_BASE);
    read_val &= ~(a << offset);
    read_val |= (val << offset);
	devmem_write(addr+GPIO_REMAP_BASE,read_val);
}
